#pragma once

#include <vector>

#include "cv_common.hpp"
#include "opencv2/opencv.hpp"

/* All calculation by [mm] */

enum class ScanMode {
  kCIECLE,
  kFRONT_180_CIRCLE,
  kFRONT_240_CIRCLE,
  kFRONT_300_CIRCLE,
  kBACK_180_CIRCLE,
  kBACK_240_CIRCLE,
  kBACK_300_CIRCLE,
};

class LidarField {
 private:
  cv::Mat map_;

  cv::Point2i drive_;
  std::vector<cv::Point2i> obstracts_;
  ScanMode mode_;

  component::LidarPoint FindDrivingPoint(const component::Nmea &coordinate);

  double CalcAttPot(const cv::Point2i &cur);
  double CalcRepPot(const cv::Point2i &cur, const double &radius);
  void CalPotField(double radius);

 public:
  LidarField(ScanMode mode = ScanMode::kCIECLE);
  ~LidarField();

  void ChangeMode(ScanMode mode);
  void SetDrivePoint(const component::Nmea &coordinate);
  void SetDrivePoint(const component::LidarPoint &coordinate);
  void SetObstracts(const std::vector<component::LidarPoint> &points);

  component::Velocity CallDirection(double radius);
  cv::Mat VisualizeObstracts(int factor);
};

cv::Mat VisualizeMeasure(cv::Mat output, std::vector<float> f);

cv::Mat VisualInner(cv::Mat output, std::vector<component::LidarPoint> pts);